#ifndef MAPPING_GROUND_FILTER_H
#define MAPPING_GROUND_FILTER_H

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/filters/filter.h>
#include <pcl/common/centroid.h>
#include <pcl/filters/extract_indices.h>

#include <Eigen/Dense>

#include <glog/logging.h>
#include <yaml-cpp/yaml.h>

namespace ground_filter{
struct PointXYZLabel
{
  PCL_ADD_POINT4D;                // quad-word XYZ
  uint16_t label;                 ///< point label
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment
} EIGEN_ALIGN16;
}
POINT_CLOUD_REGISTER_POINT_STRUCT(ground_filter::PointXYZLabel,
                                  (float, x, x)(float, y, y)(float, z, z)(uint16_t, label, label))

#define  PointXYZLabel ground_filter::PointXYZLabel 

class Ground_filter{
public:
    Ground_filter(YAML::Node& node);
    void ground_filtering(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in,pcl::PointCloud<pcl::PointXYZ>::Ptr& ground_cloud_,pcl::PointCloud<pcl::PointXYZ>::Ptr& non_ground_cloud_);
private:
    void extract_intial_seeds(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
    void estimate_plane();
    void post_process(const pcl::PointCloud<pcl::PointXYZ>::Ptr in, const pcl::PointCloud<pcl::PointXYZ>::Ptr out); 
    void remove_close_far_pt(const pcl::PointCloud<pcl::PointXYZ>::Ptr in,const pcl::PointCloud<pcl::PointXYZ>::Ptr out);
    void clip_above(const pcl::PointCloud<pcl::PointXYZ>::Ptr in,const pcl::PointCloud<pcl::PointXYZ>::Ptr out);
    static bool point_cmp(pcl::PointXYZ a, pcl::PointXYZ b)
    {
        return a.z < b.z;
    }   
private:
    double clip_height;
    double sensor_height;
    double min_distance;
    double max_distance;
    int num_iter;
    int num_lpr;
    double th_seeds;
    double th_dist; 

    float d, th_dist_d;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_backup;
    pcl::PointCloud<PointXYZLabel>::Ptr cloud_in_label;
    pcl::PointCloud<pcl::PointXYZ>::Ptr seeds_cloud;
    pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud;
    pcl::PointCloud<pcl::PointXYZ>::Ptr non_ground_cloud;

    pcl::PointCloud<pcl::PointXYZ>::Ptr final_non_ground_cloud;
    Eigen::MatrixXf normal;
};


#endif